#include "MPC5604B_M07N.h"
#include "Driver_MPC5604B.h"
//#include "camera.h"
#include "motor.h"
#include "delays.h"
#include "utils.h"
#include "driver.h"
#include <math.h>
#include <stdio.h>

vuint32_t j, gama; /* Dummy idle counter */
uint8_t Result[128], Result2[128];
uint8_t a1, a2, a3, a4, a5, a6, a7, a8, a9, a10, a11, a12, a13, a14, a15, a16, b1, b2, b3, b4, b5, b6, b7, b8, b9, b10, b11, b12, b13, b14, b15, b16; /* Read converstion result from ADC input ANS0 */
uint32_t chw, adcdata, adcdata2, curdata, max1, min1, max2, min2, th1, th2;



#define treshold 90
#define treshold2 100
#define TRUE 1
#define FALSE 0
#define HIGH 1
#define LOW 0
#define pi 3.141592
#define atraso 3
#define coef01 0.65
#define coef02 0.65
#define teste1 0
#define teste2 15
#define h 10
#define irvin 64
#define VELOCIMETER 20
#define SERVO_MCB_CHANNEL 4
#define COEFICIENTE_DE_GRAUS 1047
#define velocidadeinicial 1000
//#define velocidadeDeFreio 0
int velocidade;
//int velocidadeinicial;

//uint8_t
int i, k, d1, faixa, faixa2, d2, contador, vfinal1, vfinal2, c1, c2, anterior, anterior2, menorValor, menorValor2, atual, atual2;
double coef1, coef2, ang, ret;
int em_faixa, em_faixa2, centro[64], centro2[64], largura_c1[64], largura_c2[64];
vuint32_t total, fila[atraso];

int16_t i16Power; /* The power (in percentage) of the motor */
int32_t i32Period; /* The measured period between detections of the wheels */
int16_t i16CntrVel; /* The last scan of SAIC */
int16_t i16Overflows; /* Number of overflows of the MCB */
int16_t i16Cntr1; /* First counter used for DELAY */
int16_t i16Cntr2; /* Second counter used for DELAY */
uint16_t u16Last;
uint16_t u16Temp;
int contadorLinha[10];

//Diferencial
double dif;
double vl;
//-----------
int flag;
int TF;
int voltas;
int reta = 0;
int irvin1, irvin2, meio;
int servo;

/*void initVeloc(void) {
    SIU.PCR[68].R = 0x0100; //input!
    SIU.PCR[69].R = 0x0100; //input!
}*/

void leds(int led1, int led2, int led3, int led4) {
    if (led1 == TRUE) {
        SIU.GPDO[68].B.PDO = 0;
    }
    if (led1 == FALSE) {
        SIU.GPDO[68].B.PDO = 1;
    }
    if (led2 == TRUE) {
        SIU.GPDO[69].B.PDO = 0;
    }
    if (led2 == FALSE) {
        SIU.GPDO[69].B.PDO = 1;
    }
    if (led3 == TRUE) {
        SIU.GPDO[70].B.PDO = 0;
    }
    if (led3 == FALSE) {
        SIU.GPDO[70].B.PDO = 1;
    }
    if (led4 == TRUE) {
        SIU.GPDO[71].B.PDO = 0;
    }
    if (led4 == FALSE) {
        SIU.GPDO[71].B.PDO = 1;
    }
}

void diferencial(void) {
    int multiVL;
    int variavel;

    if (total > 1600) {

        dif = (-0.0857 * (total - 1500) + 100) / 100;
        multiVL = velocidade*dif;
        set_motores(velocidade, multiVL);
    }
    if (total < 1400) {

        dif = (0.0857 * (total - 1150) + 70) / 100;
        multiVL = velocidade*dif;
        set_motores(multiVL, velocidade);
    }
    if (total > 1400 && total < 1600) {
        set_motores(velocidade, velocidade);
    }
    variavel = 0;
}

void frenagem(void) {

    if (flag == FALSE) {
        if (total < 1300 || total > 1700) {
            if (reta > 80) {
                TF = 2 * 20; // 2=25ms
                leds(0, 0, 1, 1);
            } else {
                TF = 20;
                leds(1, 1, 0, 0);
            }
            //TF = 2 * 4;
            velocidade = 0.7 * velocidadeinicial;
            flag = TRUE;
            reta = 0;
        }
        if (total > 1250 && total < 1750) {
            reta++;
            velocidade = velocidadeinicial;
        }
    }
    if (flag == TRUE) {
        voltas++;

        if (voltas < TF) {
            freio(TRUE, TRUE);
        }
        if (voltas > TF) {
            direto(TRUE, TRUE);
            if (total > 1300 && total < 1700) {
                flag = FALSE;
                voltas = 0;
            }
        }
    }

}

void teste_freio(void) {
    int valorPWM;
    int correcao = 0;
    //	int contadorDeFreio;
    int velocidadeParaMotor;
    int abss;

    if (flag == FALSE) {
        if (total < 1250 || total > 1750) {
            if (reta > 100) {
                TF = 2 * 6; // 2=25ms   
            } else {
                TF = 8;
            }
            // TF = 2 * 5;
            flag = TRUE;
            reta = 0;
        }
        if (total > 1250 && total < 1750) {
            reta++;
        }

        flag = TRUE;
    }
    if (flag == TRUE) {
        voltas++;

        if (voltas < TF) {
            freio(TRUE, TRUE);
        }
        if (voltas > TF) {
            direto(TRUE, TRUE);
            if (total > 1250 && total < 1750) {
                flag = FALSE;
                voltas = 0;
            }
        }
    }

    abss = 1850 - total;
    if (abss < 0) {
        abss = abss * -1;
    }

    valorPWM = ((abss * 1.43) - velocidadeinicial);
    velocidadeParaMotor = velocidadeinicial - valorPWM;
    set_motores(velocidadeParaMotor, velocidadeParaMotor);
    direto(TRUE, TRUE);

}

void final_pista(void) {
    int flagao;
    //Final da pista! -------------------------------------------------
    if (c2 == 3) {
        if (largura_c2[0] >= 3 * (largura_c2[1]) - 10 || largura_c2[0] <= 3 * (largura_c2[1]) + 10) {
            flagao = TRUE;

        }
        if (largura_c2[2] >= 3 * (largura_c2[1]) - 10 || largura_c2[2] <= 3 * (largura_c2[1]) + 10) {
            flagao = TRUE;
        }
    }
    //Camera de cima vendo final de pista
    if (c1 == 3) {
        if (largura_c1[0] >= 3 * (largura_c1[1]) - 10 || largura_c1[0] <= 3 * (largura_c1[1]) + 10) {
            if (flagao == TRUE) {
                contagem_final++;
                if (contagem_final == 2) {
                    leds(TRUE, TRUE, TRUE, TRUE);
                    reverse(TRUE, TRUE);
                    DelayMEU(200);
                    freio(TRUE, TRUE);
                    DelayMEU(200);
                    break;
                }
            }

        }
        //Camera de baixo vendo final de pista.
        if (largura_c1[2] >= 3 * (largura_c1[1]) - 10 || largura_c1[2] <= 3 * (largura_c1[1]) + 10) {
            if (flagao == TRUE) {
                contagem_final++;
                if (contagem_final == 2) {
                    leds(TRUE, TRUE, TRUE, TRUE);
                    reverse(TRUE, TRUE);
                    DelayMEU(200);
                    freio(TRUE, TRUE);
                    DelayMEU(200);
                    break;
                }
            }
        }
    }
    //-----------------------------------------------------------------

}

void main(void) {
    int pinto;
    float camera1[128] = {1.50, 1.45, 1.25, 1.20, 1.15, 1.10, 1.10, 1.05, 1.05, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1.05, 1.05, 1.10, 1.10, 1.15, 1.20, 1.25, 1.45, 1.50};
    float camera2[128] = {2.31, 2.42, 2.24, 2.12, 1.95, 1.79, 1.69, 1.60, 1.56, 1.51, 1.47, 1.43, 1.43, 1.40, 1.39, 1.36, 1.34, 1.33, 1.30, 1.29, 1.28, 1.26, 1.24, 1.22, 1.21, 1.19, 1.17, 1.17, 1.15, 1.13, 1.11, 1.10, 1.11, 1.09, 1.08, 1.07, 1.07, 1.05, 1.03, 1.02, 1.01, 1, 1, 1, 1, 0.980, 0.980, 0.990, 1, 1, 1, 1, 1, 0.990, 1, 0.990, 1, 0.990, 0.980, 0.980, 0.980, 0.980, 0.980, 0.990, 0.990, 0.980, 0.980, 0.980, 0.980, 0.980, 0.990, 1, 0.990, 1, 0.990, 1, 1, 1, 1, 1, 0.990, 0.980, 0.980, 1, 1, 1, 1, 1.01, 1.02, 1.03, 1.05, 1.07, 1.07, 1.08, 1.09, 1.11, 1.10, 1.11, 1.13, 1.15, 1.17, 1.17, 1.19, 1.21, 1.22, 1.24, 1.26, 1.28, 1.29, 1.30, 1.33, 1.34, 1.36, 1.39, 1.40, 1.43, 1.43, 1.47, 1.51, 1.56, 1.60, 1.69, 1.79, 1.95, 2.12, 2.24, 2.42, 2.31};
    volatile uint32_t i = 0; /* Dummy idle counter */
    //uint8_t option;
    int contagem_final = 0;

    initModesAndClock(); /* Initialize mode entries and system clock */
    initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
    disableWatchdog(); /* Disable watchdog */

    initPads(); /* Initialize pads used in example */
    initADC(); /* Init. ADC for normal conversions but don't start yet*/
    initCTU(); /* Configure desired CTU event(s) */
    initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
    initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/

    initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
    initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
    initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
    initEMIOS_0ch5(); //antigo canal 7 modificado pelo vitor
    initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
    initEMIOS_0ch20();

    /*	initVeloc();
		
            EMIOS_0.CH[4].CBDR.R =1500;
            Delayled();
            EMIOS_0.CH[4].CBDR.R =1150;
            Delayled();
            EMIOS_0.CH[4].CBDR.R =1500;
            Delayled();
            EMIOS_0.CH[4].CBDR.R =1850;
            Delayled();
            EMIOS_0.CH[4].CBDR.R =1500;
            Delayled();*/


    init_motor();

    MOTOR_LEFT();
    MOTOR_RIGHT();
    pinto = 1500;
    leds(1,1,1,1);


    //	for (;;) 
    {
        /*
        SIU.PCR[7].R = 0x0200;				
        SIU.PCR[78].R = 0x0200;
        SIU.PCR[9].R = 0x0200;
        SIU.PCR[79].R = 0x0200;
         */
        SIU.PCR[34].R = 0x0200;
        SIU.PCR[36].R = 0x0200;
        SIU.PCR[35].R = 0x0200;
        SIU.PCR[37].R = 0x0200;


        SIU.PCR[15].R = 0x0200; /* Program the Sensor Clock pin as output*/
        SIU.PCR[14].R = 0x200;
        SIU.PCR[12].R = 0x200;



        anterior = 64;
        anterior2 = 64;
        for (;;) {
            // 	for(gama=0;gama<100;gama++)
            {


                for (j = 0; j < 2; j++) {

                    //SIU.GPDO[14].B.PDO=HIGH;
                    //	SIU.GPDO[12].B.PDO=HIGH;


                    SIU.GPDO[34].B.PDO = 0; //CK
                    SIU.GPDO[35].B.PDO = 0; //SI
                    SIU.GPDO[35].B.PDO = 1; //SI	
                    SIU.GPDO[36].B.PDO = 0; //CK
                    SIU.GPDO[37].B.PDO = 0; //SI
                    SIU.GPDO[37].B.PDO = 1; //SI
                    SIU.GPDO[15].B.PDO = 1; //PULSO
                    DelayMEU(30);
                    SIU.GPDO[34].B.PDO = 1; //CK
                    SIU.GPDO[36].B.PDO = 1; //CK
                    DelayMEU(30);
                    SIU.GPDO[35].B.PDO = 0; //SI
                    SIU.GPDO[37].B.PDO = 0; //SI
                    SIU.GPDO[15].B.PDO = 0; //PULSO
                    DelayMEU(30);
                    SIU.GPDO[34].B.PDO = 0; //CK
                    SIU.GPDO[36].B.PDO = 0; //CK


                    //	SIU.GPDO[14].B.PDO=LOW;
                    //	SIU.GPDO[12].B.PDO=LOW;
                }


                DelayMEU(30);
                //em_faixa=FALSE;
                for (i = 1; i < 128; i++) {
                    DelayMEU(30);
                    SIU.GPDO[34].B.PDO = 1; /* Sensor Clock High */
                    SIU.GPDO[36].B.PDO = 1;
                    ADC.MCR.B.NSTART = 1; /* Trigger normal conversions for ADC0 */
                    while (ADC.MCR.B.NSTART == 1) {
                    };
                    adcdata = ADC.CDR[0].B.CDATA;
                    adcdata2 = ADC.CDR[2].B.CDATA;
                    DelayMEU(30);
                    SIU.GPDO[34].B.PDO = 0; /* Sensor Clock Low */
                    SIU.GPDO[36].B.PDO = 0;
                    Result2[i] = (uint8_t) (adcdata >> 2);
                    Result[i] = (uint8_t) (adcdata2 >> 2);


                }
                //	Delaycamera();


                Delaycamera2();
            }

            em_faixa = FALSE;
            em_faixa2 = FALSE;
            c1 = 0;
            c2 = 0;
            atual = 320;
            atual2 = 320;
            max1 = 0;
            max2 = 0;
            min1 = 255;
            min2 = 255;

            for (i = 5; i < 123; i++) {
                if (Result[i] > max1) {
                    max1 = Result[i];
                }

                if (Result2[i] > max2) {
                    max2 = Result2[i];
                }

                if (Result[i] < min1) {
                    min1 = Result[i];
                }

                if (Result2[i] < min2) {
                    min2 = Result2[i];
                }


            }

            th1 = (max1 - min1) * coef01;
            th2 = (max2 - min2) * coef02;

            for (i = 0; i < 128; i++) {
                Result[i] *= camera1[i];
                if (Result[i] > max1) {
                    Result[i] = max1;
                }
                Result2[i] *= camera1[i];
                if (Result2[i] > max2) {
                    Result2[i] = max2;
                }
            }
            for (i = 10; i < 118; i++) {
                //tratamento 1
                if (Result[i] <= th1) {
                    SIU.GPDO[14].B.PDO = LOW;

                    if (em_faixa == FALSE) {
                        d1 = i;
                        em_faixa = TRUE;
                    }
                }
                if (Result[i] >= th1) {
                    SIU.GPDO[14].B.PDO = HIGH;
                    if (em_faixa == TRUE) {
                        faixa = i - d1;
                        largura_c1[c1] = faixa;
                        centro[c1] = (faixa / 2) + d1;
                        c1++;
                        em_faixa = FALSE;
                    }
                }
                //tratamento 2
                if (Result2[i] <= th2) {
                    SIU.GPDO[12].B.PDO = LOW;
                    if (em_faixa2 == FALSE) {
                        d2 = i;
                        em_faixa2 = TRUE;
                    }
                }
                if (Result2[i] >= th2) {
                    SIU.GPDO[12].B.PDO = HIGH;
                    if (em_faixa2 == TRUE) {
                        faixa2 = i - d2;
                        largura_c2[c2] = faixa2;
                        centro2[c2] = (faixa2 / 2) + d2;
                        c2++;
                        em_faixa2 = FALSE;
                    }
                }

            }


            for (i = 0; i < c1; i++) {
                if (atual > 130) {
                    menorValor = abs(centro[i] - anterior);
                    atual = menorValor;
                    vfinal1 = centro[i];

                } else {
                    atual = abs(centro[i] - anterior);
                    if (atual < menorValor) {
                        menorValor = atual;
                        vfinal1 = centro[i];

                    }

                }
            }
            anterior = vfinal1;

            for (i = 0; i < c2; i++) {
                if (atual2 > 130) {
                    menorValor2 = abs(centro2[i] - anterior2);
                    atual2 = menorValor2;
                    vfinal2 = centro2[i];
                } else {
                    atual2 = abs(centro2[i] - anterior2);
                    if (atual2 < menorValor2) {
                        menorValor2 = atual2;
                        vfinal2 = centro2[i];

                    }

                }
            }
            anterior2 = vfinal2;

            SIU.GPDO[14].B.PDO = LOW;
            SIU.GPDO[12].B.PDO = LOW;


            coef1 = (irvin - vfinal1)*0.2062;
            coef2 = (irvin - vfinal2)*0.3359;
            //coef2=(irvin-vfinal2)*0.25;
            ang = (atan((coef2 - coef1) / h)) *(180 / pi);

            //servo = tan((ang)*1.5)*150;
            //total = 1500 - servo;

            total = 1500 - (6.0345 * ang);
            //total=1500-(5.5*ang);

            if (total < 1150) {
                total = 1150;
            }

            if (total > 1850) {
                total = 1850;
            }
            //	diferencial();
            //-------------------------------------------------------------------------
            frenagem();
            diferencial();
            
            EMIOS_0.CH[4].CBDR.R = total;

            set_motores(velocidade, velocidade);

            //EMIOS_0.CH[6].CBDR.R = velocidade;
            //-------------------------------------------------------------------------
            //            if(final_pista()==TRUE){
            //                break;
            //            }
            pinto = 1500;

        }
    }

}